package Movement;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.CompassSensor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.MoveListener;
import lejos.robotics.navigation.MoveProvider;

public class NECompassPilot implements MoveProvider {

	protected CompassSensor compass;
	
	public NECompassPilot(SensorPort sensorPort) {
		CompassSensor sensor = new CompassSensor(sensorPort);
		this.compass = sensor;
		
	}
	/*
	public void rotate(float angle)
	{
		if(angle>0)
		{
			rotateRight(angle);
		}
		else{
			rotateLeft(-angle);
		}
	}
	
	public void rotateRight(float angle)
	{
		boolean rotateRight = false;
		
		if (angle > 0) {
			rotateRight = true;
			
		}else {
			angle = - angle;
			
		}
		
		Motor.B.setSpeed(100);
		Motor.A.setSpeed(100);
		
		compass.resetCartesianZero();
		float positionDept= compass.getDegrees();

		float positionArrivee;
		if(positionDept<=(359-angle))
		{
			positionArrivee=positionDept+angle;
			while(compass.getDegrees()<positionArrivee){
				if (rotateRight) {
					Motor.A.forward();
					Motor.B.backward();
					
				}else {
					
					
				}
				LCD.drawString("Boussole :"+compass.getDegrees(),0,0 );
				LCD.drawString("cartesien:"+compass.getDegreesCartesian(),1,1 );
				LCD.refresh();
				}
		}
		else{
			positionArrivee=angle-(359-positionDept);
			while(compass.getDegrees()!=positionArrivee){
				Motor.A.forward();
				Motor.B.backward();
				}
			
		}
		
		Motor.B.stop();
		Motor.A.stop();
		
	}
	
	public void rotateLeft(float angle)
	{
		Motor.B.setSpeed(100);
		Motor.A.setSpeed(100);
		compass.resetCartesianZero();
		float positionDept= compass.getDegrees();

		float positionArrivee;
		if(positionDept>=angle)
		{
			positionArrivee=positionDept-angle;
			while(compass.getDegrees()>positionArrivee){
				Motor.B.forward();
				Motor.A.backward();
				LCD.drawString("Boussole :"+compass.getDegrees(),0,0 );
				LCD.drawString("cartesien:"+compass.getDegreesCartesian(),1,1 );
				LCD.refresh();
				}
		}
		else{
			positionArrivee=359-(angle-positionDept);
			while(compass.getDegrees()!=positionArrivee){
				Motor.B.forward();
				Motor.A.backward();
				}
		}
		Motor.B.stop();
		Motor.A.stop();
		
	}*/
	
	public float getDegrees()
	{
		return this.getDegrees();
	}
	
	public void resetCartesianZero()
	{
		this.resetCartesianZero();
	}
	
	
	
	@Override
	public Move getMovement() {
		// TODO Auto-generated method stub
		return null;
	}
	@Override
	public void addMoveListener(MoveListener listener) {
		// TODO Auto-generated method stub
		
	}
	
}

